#include "tracking.h"

#define OV_THRESH 0.5

dres build_graph(dres dres_in) {
	int *f1, *f2;
	int n = dres_in.nBox, nf1, nf2, n_nei = 0;
	int inds1[1000], inds2[1000];
	f1 = new int[n];
	f2 = new int[n];

	for (int i = 0; i < n; i++) {
		neighbor nei;
		if (dres_in.fr[i] == 1) {
			dres_in.nei[n_nei] = nei;
			n_nei++;
		}
		
	}

	for (int i = 2; i <= dres_in.nFrame; i++) {
		nf1 = 0; 
		nf2 = 0;
		// Indices for detections on this frame
		int *itr1, *itr2;
		itr1 = lower_bound(&dres_in.fr[0], &dres_in.fr[n], i);
		itr2 = upper_bound(&dres_in.fr[0], &dres_in.fr[n], i);
		while (itr1 < itr2) {
			f1[nf1] = itr1-dres_in.fr;
			nf1++;
			itr1++;
		}

		// Indices for detections on the previous frame
		itr1 = lower_bound(&dres_in.fr[0], &dres_in.fr[n], i - 1);
		itr2 = upper_bound(&dres_in.fr[0], &dres_in.fr[n], i - 1);
		while (itr1 < itr2) {
			f2[nf2] = itr1 - dres_in.fr;
			nf2++;
			itr1++;
		}

		for (int j = 0; j < nf1; j++) {
			ores result;
			int n_inds1 = 0, n_inds2 = 0;
			
			result = calc_overlap(dres_in, f1[j], dres_in, f2, nf2);
			
			for (int k = 0; k < result.size; k++) 
			if (result.ov[k] > OV_THRESH) {
				inds1[n_inds1] = k;
				n_inds1++;
			}
			for (int k = 0; k < n_inds1; k++) {
				double ratio = dres_in.h[f1[j]] / dres_in.h[f2[inds1[k]]];
				inds2[n_inds2] = min(ratio, 1 / ratio) > 0.8;
				n_inds2++;
			}

			neighbor nei;
			nei.inds.reserve(n_inds2);
			nei.ovs.reserve(n_inds2);
			for (int k = 0; k < n_inds2; k++) {
				if (inds2[k] > 0) {
					//int test = f2[inds1[k]];
					nei.inds.push_back(f2[inds1[k]]);
					//nei.inds.push_back(test);
					nei.ovs.push_back(result.ov[inds1[k]]);
				}
			}
			
			dres_in.nei[n_nei] = nei;
			n_nei++;

		}
	}

	delete []f1;
	delete []f2;
	return dres_in;
}